#include "tb6612fng.h"
#include "board.h"
#include "usart.h"
#include "chassis.h"
#include "imu.h"

struct _chassis chassis;
extern eulerAngle RoboAngle;
extern struct bluetooth_Recv_Struct_t bluetooth_RecvStruct;//蓝牙控制器

/**
 * @brief 	底盘遥控刷新
 * @param   是否处于无头模式
 */
void Chassis_RemoteCtrlUpdate(bool HeadFreeMode)
{
    if(!HeadFreeMode)   //普通状态-控制器数据传给自身参考系下速度向量
    {
        chassis.selfSpeedVec[GO]    = bluetooth_RecvStruct.Rock_CH[1];
        chassis.selfSpeedVec[DRIFT] = bluetooth_RecvStruct.Rock_CH[0];
    }
    else                //无头模式-控制器数据传给世界参考系下速度向量
    {
        chassis.eulerSpeedVec[GO]    = bluetooth_RecvStruct.Rock_CH[1];
        chassis.eulerSpeedVec[DRIFT] = bluetooth_RecvStruct.Rock_CH[0];
    }

    chassis.selfSpeedVec[SPIN]  = bluetooth_RecvStruct.Rock_CH[2];//自旋始终直接传入自身参考系下速度
}


/**
 * @brief 	底盘无头模式解算
 * @param   解算输出向量 基准欧拉角 当前欧拉角
 * @note  	麦轮小陀螺模式解算
 *          输出到自身参考系下速度向量
 */
void headFree_calculate(int selfSpeedVec[3],float sta_agl,float now_agl)
{
    float agl_d = now_agl - sta_agl;

    selfSpeedVec[GO]    = chassis.eulerSpeedVec[GO] * mx_sin(agl_d);
    selfSpeedVec[GO]    -= chassis.eulerSpeedVec[DRIFT] * my_cos(agl_d);

    selfSpeedVec[DRIFT] = chassis.eulerSpeedVec[GO] * my_cos(agl_d);
    selfSpeedVec[DRIFT] += chassis.eulerSpeedVec[DRIFT] * mx_sin(agl_d);
}



/**
 * @brief 	麦轮基础解算
 * @param   自身参考系下速度向量
 * @note   结果输出到chassis.WheelSpeed[4]
 */
void mecanum_calculate(int selfSpeedVec[3])
{
    //进退解算
    chassis.WheelSpeed[WheelA] = selfSpeedVec[GO];
    chassis.WheelSpeed[WheelB] = selfSpeedVec[GO];
    chassis.WheelSpeed[WheelC] = selfSpeedVec[GO];
    chassis.WheelSpeed[WheelD] = selfSpeedVec[GO];

    //平移解算
    chassis.WheelSpeed[WheelA] += selfSpeedVec[DRIFT];
    chassis.WheelSpeed[WheelB] -= selfSpeedVec[DRIFT];
    chassis.WheelSpeed[WheelC] -= selfSpeedVec[DRIFT];
    chassis.WheelSpeed[WheelD] += selfSpeedVec[DRIFT];
    
    //自旋解算
    chassis.WheelSpeed[WheelA] += selfSpeedVec[SPIN];
    chassis.WheelSpeed[WheelB] += selfSpeedVec[SPIN];
    chassis.WheelSpeed[WheelC] -= selfSpeedVec[SPIN];
    chassis.WheelSpeed[WheelD] -= selfSpeedVec[SPIN];
}



/**
 * @brief 	底盘电机控制数据刷新
 * @note    主要函数
 */
void ChassisSpeed_Update(void)
{
    if(chassis.HeadFreeMode - chassis.LastMode > 0)//进入无头模式时记录此时世界坐标下的Yaw坐标并以此为基准
        chassis.staEulerYaw = RoboAngle.yaw;

    /**
     * 如果为普通模式，遥控数据直接传入自身参考系下速度向量
     * 如果为无头模式，遥控数据先传入世界坐标系下速度向量
     *                  经无头模式解算后再传入自身参考系下速度向量
     */
    Chassis_RemoteCtrlUpdate(chassis.HeadFreeMode);//遥控数据更新

    if(chassis.HeadFreeMode)
        headFree_calculate(chassis.selfSpeedVec,chassis.staEulerYaw,RoboAngle.yaw);
    /****自身参考系下速度向量解算完毕****/
    mecanum_calculate(chassis.selfSpeedVec);//由自身参考系下的速度向量解算出单个底盘电机的转速转向
    
    chassis.LastMode = chassis.HeadFreeMode;
    chassis.LastDeforce = chassis.deforce;
    
    /*↑↑↑↑↑↑↑↑↑↑↑数据处理↑↑↑↑↑↑↑↑↑↑↑*/
    /*↓↓↓↓↓↓↓↓↓↓↓电机控制↓↓↓↓↓↓↓↓↓↓↓*/
    if(chassis.LastDeforce - chassis.deforce > 0)//上力
        Chassis_onforce();
    else if(chassis.LastDeforce - chassis.deforce < 0)//脱力
        Chassis_deforce();

    if(!chassis.deforce)//非脱力状态下进行控制
    {
        if(chassis.Stop)
            Chassis_Stop();
        else
            Chassis_SpeedSet(chassis.WheelSpeed);
    }
}
